#include "pid.h"

// 位置式pid算法初始化
void pid_pos_init(pid_pos_typedef* pid, float kp, float ki, float kd)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->result = 0;
    pid->err_last = 0;
    pid->err_sum = 0;
    pid->max_out = 4500.0;
    pid->min_out = -4500.0;
}

// 位置式pid算法计算
float pid_pos_calc(pid_pos_typedef* pid, float currVal, float objVal)
{
    
    float err_c = objVal - currVal;       // 当前误差
    pid->err_sum += err_c;                  // 误差累计
    pid->result = pid->kp * err_c + pid->ki * pid->err_sum + pid->kd * (err_c - pid->err_last);
    pid->err_last = err_c;
    if( pid->result > pid->max_out ){
        pid->result = pid->max_out;
    }
    if( pid->result < pid->min_out ){
        pid->result = pid->min_out;
    }
    if( pid->result <=300.0 && pid->result >= -300.0 ){
        pid->result = 0;
    }
    else if( pid->result <=600.0 && pid->result >= -600.0 ){
        pid->result = pid->result/8.0;
    }
    return pid->result;
    
}



// // 增量式pid算法初始化
// void pid_delta_init(pid_delta_typedef* pid, float kp, float ki, float kd)
// {
//     pid->kp = kp;
//     pid->ki = ki;
//     pid->kd = kd;
//     pid->result = 0;
//     pid->err_last = 0;
//     pid->err_pree = 0;
// }

// // 增量式算法计算
// float pid_delta_calc(pid_delta_typedef* pid, float currVal)
// {
//     float err_c;            // 当前误差
//     float err_p;            // p误差
//     float err_i;            // i误差
//     float err_d;            // d误差
//     float increment;        // 增量
//     float objVal;           // 目标值
//     float max_out = 4000.0;
//     float min_out = -4000.0;

//     if( currVal < 180.0 ){
//         objVal = 0.0;
//     }
//     else{
//         objVal = 360.0;
//     }
//     err_c = objVal - currVal;
//     err_p = err_c - pid->err_last;
//     err_i = err_c;
//     err_d = err_c - 2 * pid->err_last + pid->err_pree;
//     increment = pid->kp * err_p + pid->ki * err_i + pid->kd * err_d;

//     pid->err_pree = pid->err_last;
//     pid->err_last = err_c;
//     pid->result += increment;
//     if( pid->result > max_out ){
//         pid->result = max_out;
//     }
//     if( pid->result < min_out ){
//         pid->result = min_out;
//     }
//     return pid->result;
// }


